#include "decoder_functions.h"

// Forney: Decoder Function
int forney(int m, Matrix * add_one, Vector *Xinv, Vector *X, int kk, Vector *Om, Vector *Lam){
   // Scratch variables
   int Xk_inv, Xk, length, Lambda_prime, Om_Xk_inv;
   int i, numerator, e;
   Vector* odds, *Lam_temp;

   // Copy input vector to avoid modifying original
   Lam_temp = copy_vector(Lam);
   
   Xk_inv = Xinv->data[kk-1];
   Xk = X->data[kk-1];

   // Calculate formal derivative (Remove even powers, replace with odd powers
   // shifted down)
   length = Lam->len;
   // Is Lam even or odd?
   if (length%2 == 0){
     // Create an every-other vector of proper length
     odds = allocate_vector(length/2);

     // Get odds
     for(i=1;i<length;i+=2){
       odds->data[(i-1)/2]=Lam->data[i];
     }
   }
   else{
     // Create an every other vector of proper length
     odds = allocate_vector((length+1)/2);
     
     // Get odds
     for(i=1;i<length;i+=2){
        odds->data[(i-1)/2]=Lam->data[i];
     }
     odds->data[(i-1)/2]= -1;
   }

   // Change data in Lam
   for(i=0;i<odds->len;i++){
      Lam_temp->data[2*i]=odds->data[i];
   }
   for(i=1;i<length;i+=2){
      Lam_temp->data[i]=-1;
   }

   free_vector(odds);

   Lambda_prime = polyPlugIn(m, add_one, Lam_temp, Xk_inv);

   free_vector(Lam_temp);

   // Evaluate Omega(Xk_inv)
   Om_Xk_inv = polyPlugIn(m, add_one, Om, Xk_inv);

   // Compute numerator
   numerator = elementMult(m, add_one, Xk, Om_Xk_inv); 

   // Compute e
   e = elementDiv(m, add_one, numerator, Lambda_prime);

   // Return result
   return(e);
}

// Euclid - serial decode function
// Precondition: Takes in the field size, add one table, number of errors to correct
//		and code word to correct
// Postcondition: Returns a decoded code word
Vector* euclid(int m, Matrix *add_one, int t, Vector *r){
  // Variables to perform the decode
  Vector *S, *X, *t_next, *t0, *t1, *rts, *rts_inv, *weights, *err_poly, *terms;
  Vector *temp, *T;
  int kk, stopiter, err;
  int i, j;
  Vector *r0, *r1;
  Vector **next;

  // TOWER 1 in parallel version
  // Create vector S (Syndrome Calculation)
  S = allocate_vector(2*t);
  for(i=0;i<2*t; i++){
    S->data[i] = polyPlugIn(m, add_one, r, i+1);
  }

  // TOWER 2 in parallel version
  // Euclid's Algorithm
  kk = 1;
  stopiter = 0;
  // Allocate r0 vector and fills with -1 ending with 0
  r0 = allocate_vector(2*t+2);
  for(i=0;i<2*t+1; i++){
    r0->data[i]=-1;
  }
  r0->data[i] = 0;

  // Allocate r1 vector and fill with 0 followed by S vector
  r1 = allocate_vector(2*t+1);
  r1->data[0] = 0;
  for(i=1;i<2*t+1;i++){
    r1->data[i] = S->data[i-1];
  }

  free_vector(S);

  // Initialize t0 and t1
  t0 = allocate_vector(1);
  t0->data[0] = -1;
  t1 = allocate_vector(1);
  t1->data[0] = 0;

  while(kk > 0){
    // Calculate r0/r1 = next[0] + next[1]
    next = polyDiv(m, add_one, r0, r1);
    X = polyMult(m, add_one, next[0], t1);
    t_next = polyAdd(m, add_one, t0, X);
    
    free_vector(X);
    free_vector(next[0]);

    temp = polyTrimZeros(next[1]);
    free_vector(next[1]);
    next[1] = temp;

    temp = polyTrimZeros(t_next);
    free_vector(t_next);
    t_next = temp;

    if(next[1]->len <= t_next->len){
      stopiter = 1;
    }
    
    // Update Indices: r0 ->r1, r1 ->rnext, t0 -> t1, t1 -> t_next
    free_vector(r0);
    r0 = r1;
    r1 = next[1];
    free(next);
    free_vector(t0);
    t0 = t1;
    t1 = t_next;

    if(stopiter == 1){
      kk = 0;
    }
 
  }  

  // Check for no error case (vector passed in was correct)  
  err = 0;
  for(i=0;i<(t1->len-1);i++){
    if(t1->data[i] != -1){
      err = 1;
      break;
    }
  }

  if(((err ==0)&&(t1->data[t1->len-1]==0))){
    T = copy_vector(r);
    free_vector(t0);
    free_vector(t1);
    free_vector(r0);
    free_vector(r1);
    return(T); // Return if there is no error
  }

  // Find roots of t1
  rts = findRoots(m, add_one,t1);

  // Calculate the inverses of the roots of t1
  // Error Locations
  rts_inv = allocate_vector(rts->len);
  for(i=0;i<rts->len;i++){
     rts_inv->data[i] = 0;
  }
  
  for(i=0;i<rts->len;i++){
    if(rts->data[i] != -1){
      rts_inv->data[i] = elementDiv(m,add_one,0, rts->data[i]);
    }
  }

  // TOWER 3 in parallel version
  // Use Forney's Algorithm to calculate the error magnitudes
  weights = allocate_vector(rts_inv->len);
  for(i=0;i<rts_inv->len;i++){ 
     weights->data[i] = 0;
  }
  for(i=0;i<rts->len;i++){
     weights->data[i] = forney(m, add_one, rts, rts_inv, i+1, r1, t1);
  }

  // Now that we know the error locations and magnitudes, generate the
  // error polynomial
  err_poly = allocate_vector(1);
  err_poly->data[0]=-1;
  for(i=0;i<rts->len;i++){
    terms = allocate_vector(rts_inv->data[i]+1);
    for(j=0;j<rts_inv->data[i];j++){
      terms->data[j] = -1;
    }
    terms->data[j] = weights->data[i];

    temp = polyAdd(m, add_one, err_poly, terms);
    free_vector(err_poly);
    free_vector(terms);
    err_poly = temp;
    
  }

  // Add the error polynomial to the received word to get the transmitted
  // code word
  T = polyAdd(m, add_one, err_poly, r);

  // Free vectors used in decode
  free_vector(err_poly);
  free_vector(rts);
  free_vector(rts_inv);
  free_vector(weights);
  free_vector(t0);
  free_vector(t1);
  free_vector(r0);
  free_vector(r1);

  // Return decoded code word
  return(T);
  
}

// Parallel version towers:

// Power Sums Tower:
// Precondition: Code word available to decode
// Postcondition: Returns syndromes for next tower
void* powerSumsTower(void* pParam){
  // Get input parameters
  powerSumsParams *params = (powerSumsParams *) pParam; 
  // Temporary variables
  int i;
  int exitVal;

  do{
    // Wait for system to become busy or exit to be triggered
    while((pBusy==0)&&(pExit == 0));

	// If exit is not triggered
    if(pExit == 0){
	    // Allocate syndrome vector
	    params->S = allocate_vector(2*(params->t));

	    // Create syndrome vector data
	    for(i=0;i<(params->S->len);i++){
	      params->S->data[i] = polyPlugIn(params->m, params->add_one, params->r, i+1);
	    }
    }

	// Reset busy flag and get exit status
    pthread_mutex_lock(&pStart);
    pBusy = 0;
    exitVal = pExit;
    pthread_mutex_unlock(&pStart);

  }while(exitVal == 0);

  // Exit from the thread
  pthread_exit(NULL);
}
  
// Sugiyama Tower:
// Precondition: Location of errors is known
// Postcondition: Calculate error magnitudes
void* sugiyamaTower(void *sParam){
  // Get input parameters
  sugiyamaParams *params = (sugiyamaParams *) sParam; 
  // Variables for decode
  Vector *r0, *t0, *X, *t_next, *temp;
  Vector **next;
  int kk, stopiter, i, err;
  int exitVal;

  do{
    // While the tower is idle and exit is not set wait
    while((sBusy==0)&&(sExit==0));

	// If exit is not set perform sugiyama tower
    if(sExit == 0){
	    // Euclid's Algorithm
	    kk = 1;
	    stopiter = 0;
	    // Allocate r0 vector and fills with -1 ending with 0
	    r0 = allocate_vector(2*(params->t)+2);
	    for(i=0;i<2*(params->t)+1; i++){
	      r0->data[i]=-1;
	    }
	    r0->data[i] = 0;

	    // Allocate r1 vector and fill with 0 followed by S vector
	    params->r1 = allocate_vector(2*(params->t)+1);
	    params->r1->data[0] = 0;

	    for(i=1;i<2*(params->t)+1;i++){
	      params->r1->data[i] = params->S->data[i-1];
	    }

	    // Initialize t0 and t1
	    t0 = allocate_vector(1);
	    t0->data[0] = -1;
	    params->t1 = allocate_vector(1);
	    params->t1->data[0] = 0;

	    while(kk > 0){
	      // Calculate r0/r1 = next[0] + next[1]
	      next = polyDiv(params->m, params->add_one, r0, params->r1);
	      X = polyMult(params->m, params->add_one, next[0], params->t1);
	      t_next = polyAdd(params->m, params->add_one, t0, X);
	    
	      free_vector(X);
	      free_vector(next[0]);

	      temp = polyTrimZeros(next[1]);
	      free_vector(next[1]);
	      next[1] = temp;

	      temp = polyTrimZeros(t_next);
	      free_vector(t_next);
	      t_next = temp;

	      if(next[1]->len <= t_next->len){
	        stopiter = 1;
	      }
	    
	      // Update Indices: r0 ->r1, r1 ->rnext, t0 -> t1, t1 -> t_next
	      free_vector(r0);
	      r0 = params->r1;
	      params->r1 = next[1];
	      free(next);
	      free_vector(t0);
	      t0 = params->t1;
	      params->t1 = t_next;

	      if(stopiter == 1){
	        kk = 0;
	      }
	    }  

	    // Check to see if input code word was correct
	    err = 0;
	    for(i=0;i<(params->t1->len-1);i++){
	      if(params->t1->data[i] != -1){
	        err = 1;
	        break;
	      }
	    }

	    if(((err ==0)&&(params->t1->data[params->t1->len-1]==0))){
	      params->correctFlag = 1; // Correct!
	    }
	    else{
	      params->correctFlag = 0; // Incorrect :(
	      // Find roots of t1
	      params->rts = findRoots(params->m, params->add_one,params->t1);

	      // Calculate the inverses of the roots of t1
	      // Error Locations
	      params->rts_inv = allocate_vector(params->rts->len);
	      for(i=0;i<(params->rts->len);i++){
	         params->rts_inv->data[i] = 0;
	      }
	  
	      for(i=0;i<(params->rts->len);i++){
	        if((params->rts->data[i]) != -1){
	          params->rts_inv->data[i] = elementDiv(params->m,params->add_one,0, params->rts->data[i]);
	        }
	      }
	    }

	    free_vector(r0);
	    free_vector(t0);
    }

    // Reset busy flag and check exit status
    pthread_mutex_lock(&sStart);
    sBusy = 0;
    exitVal = sExit;
    pthread_mutex_unlock(&sStart);

  }while(exitVal ==0);

  // Exit from the thread
  pthread_exit(NULL);
  
}

// Correction Tower:
// Precondition: Location and magnitude of errors is known
// Postcondition: Correct errors
void* correctionTower(void *cParam){
  // Get input parameters
  correctionParams *params = (correctionParams *) cParam; 
  // Variables needed in decode
  Vector *weights, *err_poly, *terms, *temp;
  int i, j;
  int exitVal;

  do{
    // Wait while tower is idle and exit is not triggered
    while((cBusy==0)&&(cExit == 0));

	// If exit is not triggered perform correction function
    if(cExit == 0){
		// If word is correct do not try to correct
	    if(params->correctFlag){
	      params->T = copy_vector(params->r);
	    }
	    else{
	      // Use Forney's Algorithm to calculate the error magnitudes
	      weights = allocate_vector(params->rts_inv->len);

	      for(i=0;i<(params->rts_inv->len);i++){ 
	        weights->data[i] = 0;
	      }
	  
	      for(i=0;i<(params->rts->len);i++){
	        weights->data[i] = forney(params->m, params->add_one, params->rts, params->rts_inv, i+1, params->r1, params->t1);
	      }

	      // Now that we know the error locations and magnitudes, generate the
	      // error polynomial
	      err_poly = allocate_vector(1);
	      err_poly->data[0]=-1;
	      for(i=0;i<(params->rts->len);i++){
	        terms = allocate_vector(params->rts_inv->data[i]+1);
	        for(j=0;j<(params->rts_inv->data[i]);j++){
	          terms->data[j] = -1;
	        }
	        terms->data[j] = weights->data[i];

	        temp = polyAdd(params->m, params->add_one, err_poly, terms);
	        free_vector(err_poly);
	        free_vector(terms);
	        err_poly = temp;
	      }

	      // Add the error polynomial to the received word to get the transmitted
	      // code word
	      params->T = polyAdd(params->m, params->add_one, err_poly, params->r);
	  
	      free_vector(err_poly);
	      free_vector(weights);
	    }
    }
	// Reset busy flag and check exit status
    pthread_mutex_lock(&cStart);
    cBusy = 0;
    exitVal = cExit;
    pthread_mutex_unlock(&cStart);

  }while(exitVal == 0);
  
  // Exit from thread
  pthread_exit(NULL);
}

// Control thread of tower pipeline
// Precondition: Array of code words is present for decode
// Postcondition: Array fully decoded via parallel pipeline
void* controlThread(void *parameters){
   // Get input parameters
   controlParams *params = (controlParams *) parameters; 
   // Temporary vectors for transferring data between towers
   Vector *S1;
   Vector *r11, *t11, *rts1, *rts_inv1;
   int i = params->pipeDepth;
   int maxVal = params->pipeDepth;
   
   // Step 1 - Power Sums Only
   // Setup power sums tower parameters
   params->pParams->m = params->m;
   params->pParams->t = params->t;
   params->pParams->add_one = params->add_one;
   params->pParams->r = params->codeArray[0];
   
   // Start power sums tower
   pthread_mutex_lock(&pStart);
   pBusy = 1;
   pthread_mutex_unlock(&pStart);
   
   // Number of steps remaining
   i--;

   // Wait for power sums to finish
   while(pBusy == 1);

   // Step 2 - Power Sums and Sugiyama
   // Setup power sums tower parameters
   params->pParams->r = params->codeArray[1];
   S1 = copy_vector(params->pParams->S);
   free_vector(params->pParams->S);

   // Setup sugiyama tower parameters
   params->sParams->m = params->m;
   params->sParams->t = params->t;
   params->sParams->add_one = params->add_one;
   params->sParams->S = S1;

   // Decrement number of steps remaining
   i--;

   // Start power sums tower
   pthread_mutex_lock(&pStart);
   pBusy = 1;
   pthread_mutex_unlock(&pStart);

   // Start sugiyama tower
   pthread_mutex_lock(&sStart);
   sBusy = 1;
   pthread_mutex_unlock(&sStart);

   while((pBusy == 1)||(sBusy == 1));

   // Step 3: All towers running
   params->cParams->m = params->m;
   params->cParams->add_one = params->add_one;

   for(i=maxVal-2;i>0;i--){
     // Setup power sums tower
     params->pParams->r = params->codeArray[maxVal-i];
     free_vector(S1);
     S1 = copy_vector(params->pParams->S);
     free_vector(params->pParams->S);

     // Setup sugiyama tower
     if((i<maxVal-2) && (params->cParams->correctFlag==0)){
       free_vector(r11);
       free_vector(t11);
       free_vector(rts_inv1);
       free_vector(rts1);
     }

     if(params->sParams->correctFlag==0){
       r11 = copy_vector(params->sParams->r1);
       t11 = copy_vector(params->sParams->t1);
       rts1 = copy_vector(params->sParams->rts);
       rts_inv1 = copy_vector(params->sParams->rts_inv);
       free_vector(params->sParams->rts);
       free_vector(params->sParams->rts_inv);
     }
     free_vector(params->sParams->r1);
     free_vector(params->sParams->t1);

     params->sParams->S = S1;

     // Setup correction tower
     params->cParams->rts = rts1;
     params->cParams->rts_inv = rts_inv1;
     params->cParams->r1 = r11;
     params->cParams->t1 = t11;
     params->cParams->correctFlag = params->sParams->correctFlag;
     params->cParams->r = params->codeArray[maxVal-i-2];

     // Start all towers
     // Start power sums tower
     pthread_mutex_lock(&pStart);
     pBusy = 1;
     pthread_mutex_unlock(&pStart);

     // Start sugiyama tower
     pthread_mutex_lock(&sStart);
     sBusy = 1;
     pthread_mutex_unlock(&sStart);

     // Start correction tower
     pthread_mutex_lock(&cStart);
     cBusy = 1;
     pthread_mutex_unlock(&cStart);

     // Wait until all complete
     while((pBusy == 1)||(sBusy == 1)||(cBusy == 1));
     
     params->Result[maxVal-i-2]=copy_vector(params->cParams->T);
     free_vector(params->cParams->T);
   }

   // Setup Sugiyama tower for last time
   free_vector(S1);
   S1 = copy_vector(params->pParams->S);
   free_vector(params->pParams->S);

   if(params->cParams->correctFlag==0){
     free_vector(r11);
     free_vector(t11);
     free_vector(rts_inv1);
     free_vector(rts1);
   }
   if(params->sParams->correctFlag==0){
	 r11 = copy_vector(params->sParams->r1);
     t11 = copy_vector(params->sParams->t1);
     rts1 = copy_vector(params->sParams->rts);
     rts_inv1 = copy_vector(params->sParams->rts_inv);
     free_vector(params->sParams->rts);
     free_vector(params->sParams->rts_inv);
   }
   params->sParams->S = S1;
   free_vector(params->sParams->r1);
   free_vector(params->sParams->t1);

   // Setup correction tower
   params->cParams->rts = rts1;
   params->cParams->rts_inv = rts_inv1;
   params->cParams->r1 = r11;
   params->cParams->t1 = t11;
   params->cParams->r = params->codeArray[maxVal-2];
   params->cParams->correctFlag = params->sParams->correctFlag;

   // Start sugiyama tower
   pthread_mutex_lock(&sStart);
   sBusy = 1;
   pthread_mutex_unlock(&sStart);

   // Start correction tower
   pthread_mutex_lock(&cStart);
   cBusy = 1;
   pthread_mutex_unlock(&cStart);

   // Wait until all complete
   while((sBusy == 1)||(cBusy == 1));
     
   params->Result[maxVal-2]=copy_vector(params->cParams->T);
   free_vector(params->cParams->T);

   // Setup Correction tower for last time
   free_vector(S1);
   if(params->cParams->correctFlag ==0){
     free_vector(r11);
     free_vector(t11);
     free_vector(rts_inv1);
     free_vector(rts1);
   }
   if(params->sParams->correctFlag==0){
     r11 = copy_vector(params->sParams->r1);
     t11 = copy_vector(params->sParams->t1);
     rts1 = copy_vector(params->sParams->rts);
     rts_inv1 = copy_vector(params->sParams->rts_inv);
     free_vector(params->sParams->rts);
     free_vector(params->sParams->rts_inv);
   }
   free_vector(params->sParams->r1);
   free_vector(params->sParams->t1);

   // Setup correction tower
   params->cParams->rts = rts1;
   params->cParams->rts_inv = rts_inv1;
   params->cParams->r1 = r11;
   params->cParams->t1 = t11;
   params->cParams->r = params->codeArray[maxVal-1];
   params->cParams->correctFlag = params->sParams->correctFlag;

   // Start correction tower
   pthread_mutex_lock(&cStart);
   cBusy = 1;
   pthread_mutex_unlock(&cStart);

   // Wait until all complete
   while(cBusy == 1);
     
   params->Result[maxVal-1]=copy_vector(params->cParams->T);
   free_vector(params->cParams->T);   

   if(params->cParams->correctFlag ==0){
     free_vector(r11);
     free_vector(t11);
     free_vector(rts_inv1);
     free_vector(rts1);  
   }

   pthread_mutex_lock(&cStart);
   cExit = 1;
   pthread_mutex_unlock(&cStart);

   pthread_mutex_lock(&pStart);
   pExit = 1;
   pthread_mutex_unlock(&pStart);

   pthread_mutex_lock(&sStart);
   sExit = 1;
   pthread_mutex_unlock(&sStart);

   pthread_exit(NULL);

}

Vector** parallelTowers(int m, Matrix *add_one, int t, int pipeDepth, Vector** codeArray){
    Vector** result = (Vector**)malloc(sizeof(Vector*)*pipeDepth);
    pthread_t thread[4];
    powerSumsParams  pParam;
    sugiyamaParams   sParam;
    correctionParams cParam;
    controlParams parameters;
    int err;

    // Init mutexes
    pthread_mutex_init(&pStart,NULL);
    pthread_mutex_init(&sStart,NULL);
    pthread_mutex_init(&cStart,NULL);
    pBusy = 0;
    sBusy = 0;
    cBusy = 0;
    pExit = 0;
    sExit = 0;
    cExit = 0;

    parameters.m = m;
    parameters.t = t;
    parameters.pipeDepth = pipeDepth;
    parameters.add_one = add_one;
    parameters.codeArray = codeArray;
    parameters.pParams = &pParam;
    parameters.sParams = &sParam;
    parameters.cParams = &cParam;
    parameters.Result = result;

    // Create control thread
    err = pthread_create(&thread[0], NULL, controlThread, (void *) &parameters);
    if(err!=0)
      fprintf(stderr, "ERROR: Control pthread did not create!\n");

    // Create first tower - syndrome return in S
    err = pthread_create(&thread[1], NULL, powerSumsTower, (void *) &pParam);
    if(err!=0)
      fprintf(stderr, "ERROR: Power Sums Tower pthread did not create!\n");

    // Create Second tower
    err = pthread_create(&thread[2],NULL,sugiyamaTower,(void *) &sParam);
    if(err!=0)
      fprintf(stderr, "ERROR: Sugiyama Tower pthread did not create!\n");

    // Create Third tower
    err = pthread_create(&thread[3],NULL,correctionTower,(void*) &cParam);  
    if(err!=0)
      fprintf(stderr, "ERROR: Correction Tower pthread did not create!\n");

    // Wait for pipeline to finish
    while((cExit == 0)||(pExit == 0)||(sExit == 0));

    //pthread_exit(parameters.Result);
	return(parameters.Result);
}
